/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef CJanRoutePlanner_H_
#define CJanRoutePlanner_H_
#include <vector>
#include "ntree.hpp"

#define           jDRP_type     float
struct TPathPixel{
	public:
		int x;
		int y;
		TPathPixel(){
			x=0;
			y=0;
		}
		TPathPixel(int _x,int _y){
			x = _x; y=_y;
		}
};

class CJanRoutePlanner{
	public:
		CJanRoutePlanner(float *in,int w,int h){
			int c1;
			jDRP_w=w;
			jDRP_h=h;
			jDRP_numel=jDRP_w*jDRP_h;
			jDRP_dens=new jDRP_type[jDRP_numel];  
			jDRP_dist=new jDRP_type[jDRP_numel];
			jDRP_pred=new int[jDRP_numel];
			jDRP_mask=new int[jDRP_numel];
  
			for (c1=0;c1<jDRP_numel;c1++) {
				jDRP_dens[c1]=in[c1];
			}
		}
		
		~CJanRoutePlanner(){
			delete[] jDRP_dens;
			delete[] jDRP_dist;
			delete[] jDRP_pred;
			delete[] jDRP_mask; 
		}
		
		bool plan(TPathPixel from, TPathPixel to, std::vector<TPathPixel> &route){
			int tok;
			
			jDRP_routeFrom(from.x,from.y,10000.0f);
			//jDRP_planTo   (from.x,from.y);        
			
			
			jDRP_planTo   (to.x,to.y);        
			//jDRP_routeFrom(to.x,to.y,10000.0f);
			
			route.clear();
			while (jDRP_plan.token(tok)) {
				//fprintf(stderr,"(%d, %d)\n",tok%jDRP_w,tok/jDRP_w);
				route.push_back(TPathPixel(tok%jDRP_w,tok/jDRP_w));
			}
			
			return true;
		}
	 
		
	private:
		int               jDRP_w;               // read only
		int               jDRP_h;               // read only
		linked<int>       jDRP_plan;            // read only

// private
		jDRP_type*        jDRP_dens;
		jDRP_type*        jDRP_dist;
		int*              jDRP_pred;
		int*              jDRP_mask;
		int               jDRP_numel;

		void jDRP_routeFrom(int pix,int piy,jDRP_type lim) {
			int c1;//,c2,c3,c4;
			int pia,pii;  
			for (c1=0;c1<jDRP_numel;c1++) {
				jDRP_dist[c1]=jDRP_numel;
				jDRP_pred[c1]=0;
				jDRP_mask[c1]=0;
			}
  // BEGIN: do fancy shortest-path algorithm
			pia=piy*jDRP_w+pix;                               // map the robot into the map  
			linked<int>  list0,list1;
			linked<int>* now=&list0;
			linked<int>* nxt=&list1;
			linked<int>* tmp;
			now->append(pia);
			jDRP_dist[pia]=0;
			jDRP_pred[pia]=pia;
			jDRP_type one=1,rt2=sqrt(2.0),rt5=sqrt(5.0);
  
  //int       ofs[8]={-jDRP_w-1,-jDRP_w+1, jDRP_w+1, jDRP_w-1,  1,-jDRP_w, -1,jDRP_w};
  //jDRP_type fac[8]={ rt2     , rt2     , rt2     , rt2     ,one, one   ,one,one   };

			int       ofs[16]={-jDRP_w-jDRP_w-1,-jDRP_w-jDRP_w+1,-jDRP_w-2,-jDRP_w+2,jDRP_w-2,jDRP_w+2,jDRP_w+jDRP_w-1,jDRP_w+jDRP_w+1, -jDRP_w-1,-jDRP_w+1, jDRP_w+1, jDRP_w-1,  1,-jDRP_w, -1,jDRP_w};
			jDRP_type fac[16]={rt5,rt5,rt5,rt5,rt5,rt5,rt5,rt5, rt2     , rt2     , rt2     , rt2     ,one, one   ,one,one   };

			jDRP_type met;
			int cnt=0;
			while (now->next[0]&&cnt<lim) {                   // repeat the process until now is empty, or mhh...
				cnt++;
				while (now->token(pia)) {                       // in the while loop, pia contains pixel address to be processed
					for (c1=0;c1<16;c1++) {
						pii=pia+ofs[c1];
						if (0<pii&&pii<jDRP_numel)
							if ((met=jDRP_dens[pii]*fac[c1]))            // only visit explored terrain (metric =0 whereever unexplored)
					//met=jDRP_dens[pii]*fac[c1];
								if (jDRP_dist[pia]+met<jDRP_dist[pii]&&jDRP_dist[pia]+met<lim) {    // IF ROUTING DOES NOT WORK, REMOVE latter condition!!!
							jDRP_dist[pii]=jDRP_dist[pia]+met;
							jDRP_pred[pii]=pia;
							if (jDRP_mask[pii]<cnt) {
								jDRP_mask[pii]=cnt;
								nxt->append(pii);
							}
								}
					}
				}
				while (now->token(pia))                         // remove
					now->remove();
				tmp=now;
				now=nxt;
				nxt=tmp;
			}
  // END: do fancy shortest-path algorithm
		}

		jDRP_type jDRP_planTo(int pix,int piy) {
			int tok;
			int pta=piy*jDRP_w+pix;                              // pixel address of target  
			while (jDRP_plan.token(tok))                         // delete old path
				jDRP_plan.remove();
			while (jDRP_pred[pta]&&pta!=jDRP_pred[pta]) {   // hop from predecessor to next predecessor
				jDRP_plan.prepend(pta);
				pta=jDRP_pred[pta];
			}
			return jDRP_dist[pta];
		}
};

#endif
